#include "MyProject.h"

/******************************************************************************/
float shaft_angle0;
float shaft_angle;
float electrical_angle0;
float electrical_angle;
float shaft_velocity0;
float shaft_velocity;
float current_sp0;
float current_sp;
float shaft_velocity_sp0;
float shaft_velocity_sp;
float shaft_angle_sp0;
float shaft_angle_sp;
DQVoltage_s voltage0;
DQVoltage_s voltage;
DQCurrent_s current0;
DQCurrent_s current;

TorqueControlType torque_controller;
MotionControlType controller;

float sensor_offset0=0;
float sensor_offset=0;
float zero_electric_angle0;
float zero_electric_angle;
/******************************************************************************/
// shaft angle calculation
float shaftAngle0(void)
{
  // if no sensor linked return previous value ( for open loop )
  //if(!sensor) return shaft_angle;
  return sensor_direction0*getAngle0() - sensor_offset0;
}
float shaftAngle(void)
{
  // if no sensor linked return previous value ( for open loop )
  //if(!sensor) return shaft_angle;
  return sensor_direction*getAngle() - sensor_offset;
}
// shaft velocity calculation
float shaftVelocity(void)
{
  // if no sensor linked return previous value ( for open loop )
  //if(!sensor) return shaft_velocity;
  return sensor_direction*LPF_velocity(getVelocity());
}
float shaftVelocity0(void)
{
  // if no sensor linked return previous value ( for open loop )
  //if(!sensor) return shaft_velocity;
  return sensor_direction0*LPF_velocity0(getVelocity0());
}
/******************************************************************************/
float electricalAngle0(void)
{
  return _normalizeAngle((shaft_angle0 + sensor_offset0) * pole_pairs0 - zero_electric_angle0);
}
float electricalAngle(void)
{
  return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle);
}
/******************************************************************************/


